import rclpy  
from rclpy.node import Node    
from geometry_msgs.msg import Twist  
import numpy as np  
from std_msgs.msg import Int32
from origincar_msg.msg import Sign  # 根据图片的消息类型
from origincar_msg.msg import Data
import math
from pyzbar.pyzbar import decode
from math import atan2, sqrt, sin, cos
from ai_msgs.msg import PerceptionTargets
import cv2

from threading import Thread



class InertialNavigationNode(Node):  
    def __init__(self):  
        super().__init__('inertial_navigation_node_task1')  # 初始化节点名称

        self.state = 0
        self.task = 0
        
        # 创建目标位置订阅，主要用于订阅目标位置,分别是x.y和角度
        self.subscription_robotpose = self.create_subscription(
            Data,  
            '/robotpose',
            self.robotpose_callback,
            2
        )
        
        # 创建一个订阅者，订阅 /sigh4return 话题，消息类型为 Int32,上位机信号
        self.subscription_sign4return = self.create_subscription(
            Int32,
            '/sign4return',
            self.sign4return_callback,
            10) 
        
        #创建订阅者，订阅话题 hobot_dnn_detection 话题   
        self.subscription_fcos = self.create_subscription( 
            PerceptionTargets, 
            '/hobot_dnn_detection',  
            self.fcos_callback, 
            10)  
               
        #二维码识别
        self.subscription_sign_switch = self.create_subscription(  
            Sign,  
            'sign_switch',  
            self.sign_switch_callback,  
            3)
        
        
        # 创建速度命令发布者
        self.cmd_pub = self.create_publisher(Twist, '/cmd_vel', 1)  


        # 初始化状态变量用来储存位置
        self.angular =  0
        self.position = np.array([0.0, 0.0])  
        self.target_position = np.array([0.0, 0.0])  # 目标位置     

        
        #锥桶的相对坐标在这里，分别是底边中点x,y,宽度和高度
        self.zhuitong_relativ_position = np.array([0.0, 0.0,0.0,0.0])
        self.qr_relativ_position = np.array([0.0, 0.0,0.0,0.0])

    
        #40cm左右，这个坐标是距离40的相对坐标
        self.zhuitongyuzhi_40x = 250
        self.vx = 1.0
        self.vx3 = 0.85
        self.vqr = 0.5
        

        self.bizhangall = 0.0
        self.jishiqi = 0
        #创建定时器发布cmd_vel
        self.timer = self.create_timer(
            timer_period_sec=1/30,  # 时间间隔（秒）
            callback=self.timer_callback  # 回调函数
        )
                    

    def timer_callback(self):        
        if self.task == 1 :
            if(self.jishiqi > 0) :
                self.navigation_0(self.new_x,self.new_y,self.vx3)
                self.jishiqi = self.jishiqi - 1
            else:
                self.navigation_control()
            
    
    
    def fcos_callback(self, msg):
        if(self.task == 1):
            for num, target in enumerate(msg.targets):
                
                if target.rois:
                    
                    class_name = target.rois[0].type.strip()
                    # 获取第一个 ROI
                    roi = target.rois[0].rect

                    # 获取框参数
                    x_offset = roi.x_offset
                    y_offset = roi.y_offset
                    height = roi.height
                    width = roi.width
                            
                    if class_name == 'obstacle': 
                        #底边中点坐标
                        x = int(x_offset + 0.5 * width)
                        y = y_offset + height
                        zhuitong_relativ_position = x,y,width,height
                        self.get_logger().info('锥桶位置x = {:.3f} 锥桶高度={:.3f}'.format(zhuitong_relativ_position[0],zhuitong_relativ_position[1]))
                        if (y > self.zhuitongyuzhi_40x) :
                            self.state = 1    
                            self.zhuitong_relativ_position = zhuitong_relativ_position
                            self.navigation_1()

                            return

                    if class_name == 'qr': 
                        #底边中点坐标
                        x = int(x_offset + 0.5 * width)
                        y = y_offset + height
                        self.qr_relativ_position = x,y,width,height
                        self.state = 2    
                        self.navigation_qr()
                        return
                                                                        
            self.state = 0    

    def robotpose_callback(self, msg):
        # 更新机器人的x轴位置坐标
        self.position[0] = msg.x
        # 更新机器人的y轴位置坐标
        self.position[1] = msg.y
        # 更新机器人的角度
        self.angular = msg.z

        

    
    def sign_switch_callback(self,msg):
        #二维码识别的回调函数，必须要任务1的时候在回调
        if(self.task == 1):
            self.task = 2
            self.state = 0

            cmd_msg = Twist()  
            cmd_msg.linear.x = 0.0
            cmd_msg.angular.z = 0.0
            self.cmd_pub.publish(cmd_msg)  


    def sign4return_callback(self,msg):    
        
        # msg.data == 1 表示任务一开始    
        if  msg.data == -1:
            self.state = 0
            self.task = 1

            #前往二维码区
            self.target_position[0] = 4.8  # 目标x和y坐标
            self.target_position[1] = 1.7      
              
            self.get_logger().info(f"任务一")
            
        # msg.data == 2 任务二开始
        if  msg.data == 5:
            self.task = 2
            self.state = 0

            twist = Twist()
            twist.linear.x = 0.0
            twist.angular.z = 0.0
            self.cmd_pub.publish(twist)  
        
            self.get_logger().info(f"任务二")
            
        return
    
    
    def navigation_control(self): 
        
        if self.state == 0 :
            self.navigation_0(self.target_position[0],self.target_position[1],self.vx)

        if self.state == 2 :
            self.navigation_qr()
            

                                                    

    def navigation_0(self,target_x,target_y,vx): #state=0 无避障简单惯导
        # 计算目标点的角度
        dx = target_x - self.position[0]
        dy = target_y - self.position[1]
        
        target_angle_global = atan2(dy, dx)
        angle_diff = target_angle_global - self.angular
        angle_diff = atan2(sin(angle_diff), cos(angle_diff))
        
        #if(self.state == 0):
         #   if( self.bizhangall > 0 ):
                #angle_diff = angle_diff - self.bizhangall * 0.3
          #      self.bizhangall = self.bizhangall - 1
           # if( self.bizhangall < 0 ):
           #     angle_diff = angle_diff - self.bizhangall * 0.3
            #    self.bizhangall = self.bizhangall + 1


        twist = Twist()
        twist.linear.x = vx
        twist.angular.z = 1.7 * angle_diff
        self.cmd_pub.publish(twist)  

        self.get_logger().info('self.position[0] = {:.3f} self.position[1] = {:.3f}'.format(self.position[0],self.position[1]))
        self.get_logger().info('target_x = {:.3f}target_y = {:.3f}bizhangall = {}'.format(target_x,target_y,self.bizhangall))
        self.get_logger().info('self.state = {:.3f}target_angle={:.3f}'.format(self.state,angle_diff))


    def navigation_1(self): #state=1 避障导航
              
        #锥桶在右，向左避障,左转的角度比较小
        if(self.zhuitong_relativ_position[0] > 320 ):

            self.new_x = self.position[0] + 0.3 * math.cos(self.angular) - 0.2  * math.sin(self.angular)   #* 0.012 * (640 - self.zhuitong_relativ_position[0])
            self.new_y = self.position[1] + 0.3 * math.sin(self.angular) + 0.2  * math.cos(self.angular)
            
            self.jishiqi = 1
            self.navigation_0(self.new_x,self.new_y,self.vx)

            self.get_logger().info(f"任务3向左避障")
            
                   
        #锥桶在左，向右避障
        if(self.zhuitong_relativ_position[0] <= 320 ):

            self.new_x = self.position[0] + 0.3 * math.cos(self.angular) + 0.2   * math.sin(self.angular)  # * 0.012 * self.zhuitong_relativ_position[0]
            self.new_y = self.position[1] + 0.3 * math.sin(self.angular) - 0.2   * math.cos(self.angular)
            
            self.jishiqi =  1

            self.navigation_0(self.new_x,self.new_y,self.vx)

            self.get_logger().info(f"任务3向右避障")
            


    def navigation_qr(self): #state=1，识别qr，减速行驶
        cmd_msg = Twist()  
        cmd_msg.linear.x = self.vqr  # 设置线性速度  
        
        dx = self.target_position[0] - self.position[0]
        dy = self.target_position[1] - self.position[1]
        
        target_angle_global = atan2(dy, dx)
        angle_diff = target_angle_global - self.angular
        angle_diff = atan2(sin(angle_diff), cos(angle_diff))
        cmd_msg.angular.z =  1.0 *angle_diff    # 设置角速度
       
        self.get_logger().info(f"识别qr减速行驶")  
        

        self.cmd_pub.publish(cmd_msg)  # 发布控制指令


def main(args=None):  
    rclpy.init(args=args)  # 初始化rclpy  
    navigation = InertialNavigationNode()  #
    rclpy.spin(navigation)
    navigation.destroy_node()  # 销毁节点
    rclpy.shutdown()
  
if __name__ == '__main__':  
    main()  # 入口函数

